﻿using System;
using System.Collections.Generic;
using WiMo.Games.Drawables;

namespace WiMo.Games.StandardPhysics
{
    public class PhysicsManager : IPhysicsManager
    {
        private List<ISprite> _spriteList;
        public PhysicsManager()
        {
            _spriteList = new List<ISprite>();
            Gravity = Vector2.Zero;
        }

        public void Add(ISprite sprite)
        {
            _spriteList.Add(sprite);
        }

        public void Update(GameTime gameTime)
        {
            var scaleFactor = (Convert.ToSingle(gameTime.ElapsedGameTime.TotalMilliseconds) / 1000);

            foreach (var sprite in _spriteList)
            {
                if (!sprite.IsStatic)
                {
                    if (sprite.AffectedByGravity)
                    {
                        var gravity = Gravity.Clone();
                        sprite.Force += (gravity * scaleFactor);
                    }

                    if (sprite.Spring != null)
                        sprite.Spring.Update(gameTime);

                    sprite.Position = new PointF((sprite.Force.X * scaleFactor) + sprite.UnscaledPosition.X, (sprite.Force.Y * scaleFactor) + sprite.UnscaledPosition.Y);
                    sprite.Velocity = new Vector2(sprite.LastPosition - sprite.Position) / scaleFactor;
        
                    sprite.LastUpdated = gameTime.Clone();
                }
            }
      }

        public void Remove(ISprite sprite)
        {
            _spriteList.Remove(sprite);
        }

        public Vector2 Gravity { get; set; }

        public void UpdateForcesAfterCollision(ISprite sprite1, ISprite sprite2)
        {
            var delta = sprite1.GetDistance(sprite2);

            double theta1 = delta.Direction;
            double theta2 = delta.Direction;

            var v1p = (sprite1.Force.X * Convert.ToSingle(Math.Cos(theta1)) + sprite1.Force.Y * Convert.ToSingle(Math.Sin(theta1)));
            var v1n = (-sprite1.Force.X * Convert.ToSingle(Math.Sin(theta1)) + sprite1.Force.Y * Convert.ToSingle(Math.Cos(theta1)));

            var v2p = (sprite2.Force.X * Convert.ToSingle(Math.Cos(theta2)) + sprite2.Force.Y * Convert.ToSingle(Math.Sin(theta2)));
            var v2n = (-sprite2.Force.X * Convert.ToSingle(Math.Sin(theta2)) + sprite2.Force.Y * Convert.ToSingle(Math.Cos(theta2)));

            var tmp = 1 / (sprite1.Mass + sprite2.Mass);

            var e1 = sprite1.CoefRestitution;
            var e2 = sprite2.CoefRestitution;
            var m1 = sprite1.Mass;
            var m2 = sprite2.Mass;

            var v1pPrime = ((m1 - e1 * m2) * tmp * v1p) + ((1.0f + e1) * m2 * v2p * tmp);
            var v2pPrime = (((1.0f + e1) * m1 * v1p * tmp) + ((m2 - e1 * m1) * v2p * tmp));

            var v1x = (v1pPrime * Convert.ToSingle(Math.Cos(theta1)) - v1n * Convert.ToSingle(Math.Sin(theta1)));
            var v1y = (v1pPrime * Convert.ToSingle(Math.Sin(theta1)) + v1n * Convert.ToSingle(Math.Cos(theta1)));

            var v2x = (v2pPrime * Convert.ToSingle(Math.Cos(theta2)) - v2n * Convert.ToSingle(Math.Sin(theta2)));
            var v2y = (v2pPrime * Convert.ToSingle(Math.Sin(theta2)) + v2n * Convert.ToSingle(Math.Cos(theta2)));

            if (!sprite1.IsStatic)
                sprite1.Force = new Vector2(v1x, v1y);

            if (!sprite2.IsStatic)
                sprite2.Force = new Vector2(v2x, v2y);
        }

    }
}
